clc
clear

% Construct a unit quaternion object Q1 by specifying s scalar and vector parts which are normalised
S = 10;
V = [1/2, 0 ,1/2];
Q1 = UnitQuaternion(S, V);
disp(Q1);

% Construct a unit quaternion object Q2 by specifying s scalar and vector parts which are normalised
S = 10;
V = [0, 1 ,0];
Q2 = UnitQuaternion(S, V);
disp(Q2);

% Calculate Angle between two UnitQuaternions
A = Q1.angle(Q2);
disp(A);

% Animate UnitQuaternion object
% Q1.animate(10);
% Q2.animate(10);

% Convert to string
%The vector part is delimited by single angle brackets, 
%to differentiate from aQuaternion which is delimited by double angle brackets.
fprintf(Q1.char());

% UnitQuaternion derivative in world frame
omega = [1,1,1];
QD = Q1.dot(omega);
disp(QD);

S=0.5;
QI = Q1.interp(Q2, S, 'shortest');
disp(QI);

% QI.plot();
disp(Q1.inv());

% Convert unit quaternion as vector to SO(3)rotation matrix
V = [1,1/2, 0 ,1/2];
disp(UnitQuaternion.q2r(V));

% Convert to SO(3)rotation matrix
disp(Q1.R());

% Construct UnitQuaternion from rotation about y-axis
disp(UnitQuaternion.Rx(10));
disp(UnitQuaternion.Ry(10));
disp(UnitQuaternion.Rz(10));

% Convert to SE3 object
disp(Q1.SE3());

% Convert to SO3 object
disp(Q1.SO3());

% Convert to homogeneous transformation matrix
disp(Q1.T());

disp(Q1.torpy('deg'));

% Convert SO(3)or SE(3)matrix to unit quaternion as vector
R= rpy2tr(10,20,30);
disp(R);
disp(UnitQuaternion.tr2q(R));